#include "cpp_co_location_client/CoLocationNode.hpp"
// #include <functional>
// #include <memory>
// #include <thread>
#include <chrono>        // For chrono types
using namespace std::chrono_literals;  // Enable the "ms" literal and other time literals

#define MAXDATANUM 4 //每个timer时间间隔内最大接收的检测结果数量


CoLocationNode::CoLocationNode()
: Node("co_location_node")
{
    // Initialize publisher
    publisher_ = this->create_publisher<client_interfaces::msg::EstimateTargetPosition>("target_location_estimations", 10);
    timer_ = this->create_wall_timer(500ms, std::bind(&CoLocationNode::publish_estimates, this));

    // // 创建 QoS 配置
    // rclcpp::QoS qos_profile(10);  // 设置队列长度为 10
    // qos_profile.reliability(rclcpp::QoS::ReliabilityPolicy::BestEffort)  // 设置可靠性策略
    //             .history(rclcpp::QoS::HistoryPolicy::KeepLast);  // 设置历史策略
    // rclcpp::QoS(5).best_effort().keep_last(2)   rclcpp::QoS(10).best_effort().keep_last(3)  .best_effort().keep_last(5)

    // communication
    subscription_ = this->create_subscription<client_interfaces::msg::ConfigTarget>(
        "target_found",rclcpp::QoS(50).best_effort(), std::bind(&CoLocationNode::target_callback, this, std::placeholders::_1));

    origin_subscription_ = this->create_subscription<yolo_detect_message::msg::Geolocation>(
        "/uav_swarms/global_init_positon_gps",rclcpp::QoS(50).best_effort(), std::bind(&CoLocationNode::init_pos_callback, this, std::placeholders::_1));
    
    // Initialize services
    service_create_airplane_ = this->create_service<client_interfaces::srv::CreateNewAirplane>(
        "create_new_airplane", std::bind(&CoLocationNode::create_airplane, this, std::placeholders::_1, std::placeholders::_2));

    service_delete_airplane_ = this->create_service<client_interfaces::srv::DeleteAirplane>(
        "delete_airplane", std::bind(&CoLocationNode::delete_airplane, this, std::placeholders::_1, std::placeholders::_2));

    service_delete_target_ = this->create_service<client_interfaces::srv::DeleteTarget>(
        "delete_target", std::bind(&CoLocationNode::delete_target, this, std::placeholders::_1, std::placeholders::_2));

    service_change_payload_ = this->create_service<client_interfaces::srv::ChangePayload>(
        "change_payload", std::bind(&CoLocationNode::change_payload, this, std::placeholders::_1, std::placeholders::_2));

    service_get_airplanes_id_ = this->create_service<client_interfaces::srv::GetAirplanesId>(
        "get_airplanes_id", std::bind(&CoLocationNode::get_airplanes_id, this, std::placeholders::_1, std::placeholders::_2));

    service_get_targets_id_ = this->create_service<client_interfaces::srv::GetTargetsId>(
        "get_targets_id", std::bind(&CoLocationNode::get_targets_id, this, std::placeholders::_1, std::placeholders::_2));

    GlobalOrigin_.altitude = 0;
    GlobalOrigin_.longtitude = -200;
    GlobalOrigin_.latitude = -200;

    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "created msg srv . topic is published to Target_Location_Estimations. Need to Recieve target_found topic and create_new_airplane service ");
}

/**
 * @brief 发布估计的目标位置
 *
 * 该函数用于发布估计的目标位置信息。
 *
 * 首先，获取所有目标ID，并检查是否有目标需要估计。
 * 如果有目标需要估计，则获取最后一个目标ID，并更新该目标的状态。
 * 然后，获取该目标的估计结果，并将结果填充到消息中。
 * 如果已经初始化了原点信息，则将估计结果从ENU坐标系转换为WGS84坐标系。
 * 最后，发布消息，并打印估计结果。
 */
void CoLocationNode::publish_estimates()
{
    auto message = client_interfaces::msg::EstimateTargetPosition();
    std::vector<uint8_t> targets_id = AI_Co_Location_Computer.GetAllTargetsId();
    if(!targets_id.empty())
    {
        auto &target_id = targets_id.back();
        AI_Co_Location_Computer.UpdateTargetState(target_id);
        std::vector<Filtered_Target_State> estimate_results;
        if (AI_Co_Location_Computer.GetEstimate_Results_(estimate_results, target_id))
        {
            DataNum = 0;
            const auto &it = estimate_results.back();
            message.estimate_time = it.FilterTime;
            message.target_id = target_id;
            message.x = it.State(1);
            message.y = it.State(0);
            message.z = -it.State(2);

            if(origin_inited_)
            {
                double lat,lon,alt;
                enu_to_wgs84(message.x, message.y, message.z, 
                  GlobalOrigin_.latitude, GlobalOrigin_.longtitude , GlobalOrigin_.altitude,
                  lat,lon,alt);
                message.latitude = lat;
                message.longtitude = lon;
                message.altitude = alt;
            }
            
            publisher_->publish(message);
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Result is: x %f, y %f, z %f, Time is: %llu", message.x ,message.y ,message.z , it.FilterTime);
        }
    }
}

/**
 * @brief 回调函数，用于处理目标配置消息
 *
 * 该函数接收一个目标配置消息，并根据消息内容更新对应飞机的状态。
 *
 * @param message 目标配置消息
 */
void CoLocationNode::target_callback(const client_interfaces::msg::ConfigTarget::SharedPtr message)
{
    
    uint8_t airplane_id = message->airplane_id;
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Recieve Target at: %llu, airplaneID: %f, DataNum: %f\n ", message->time_measure, float(AI_Co_Location_Computer.AirplaneIsCreated(airplane_id)), float(DataNum));

    if((AI_Co_Location_Computer.AirplaneIsCreated(airplane_id)) && DataNum < MAXDATANUM)
    {
        DataNum++;
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Airplane Prepared!\n");
        Eigen::Vector3d t_n(message->body_translation_y, message->body_translation_x, -message->body_translation_z);
        Eigen::Vector3d t_c(message->camera_translation_x, message->camera_translation_y, message->camera_translation_z);
        Rotation_Angles gimbal_rotation_angles{0, message->gimbal_rotation_pitch, message->gimbal_rotation_yaw};
        Rotation_Angles body_rotation_angles{ message->body_rotation_roll, message->body_rotation_pitch, M_PI/2 - message->body_rotation_yaw};
        Picture_Location picture_location{message->picture_location_u, message->picture_location_v};
        uint64_t time_measure = message->time_measure;
        
        AI_Co_Location_Computer.UpdateAirplaneState(t_n, t_c, gimbal_rotation_angles, body_rotation_angles, message->target_id, airplane_id, picture_location, time_measure);
        RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Airplane Updated!\n");
    }
}

/**
 * @brief 初始化位置回调函数
 *
 * 接收YOLO检测消息中的地理位置信息，并初始化全局坐标原点。
 *
 * @param message 共享指针，指向YOLO检测消息中的地理位置信息
 */
void CoLocationNode::init_pos_callback(const yolo_detect_message::msg::Geolocation::SharedPtr message)
{
    if(!origin_inited_)
    {
        origin_inited_ = true;
        GlobalOrigin_.latitude = message->latitude;
        GlobalOrigin_.longtitude = message->longtitude;
        GlobalOrigin_.altitude = message->altitude;
    }
}

/**
 * @brief 创建新的飞行器节点
 *
 * 该函数实现了一个名为 `create_airplane` 的服务，用于创建新的飞行器节点。
 *
 * @param request 请求参数，包含创建新飞行器所需的所有信息。
 * @param response 响应参数，用于返回操作结果。
 */
void CoLocationNode::create_airplane(const std::shared_ptr<client_interfaces::srv::CreateNewAirplane::Request> request,
                                     std::shared_ptr<client_interfaces::srv::CreateNewAirplane::Response> response)
{
    // Implementation of create_airplane service
    // ...
    uint8_t AirplaneID = request->airplane_id;
    Eigen::Vector3d t_g;
    t_g(0) = request->pod_translation_x;
    t_g(1) = request->pod_translation_y;
    t_g(2) = request->pod_translation_z;
    Rotation_Angles Pod_Rotation_Angles;
    Pod_Rotation_Angles.pitch = request->pod_rotation_pitch;
    Pod_Rotation_Angles.roll = request->pod_rotation_roll;
    Pod_Rotation_Angles.yaw = request->pod_rotation_yaw;
    Gimbal_Camera Gimbal_camera_parameter;
    // Gimbal_camera_parameter.d = request->pixel_distance;//0.0000135156;
    // Gimbal_camera_parameter.f = request->focal_distance;//0.014722;
    Gimbal_camera_parameter.fx = request->fx;
    Gimbal_camera_parameter.fy = request->fy;
    Gimbal_camera_parameter.cx = request->cx;
    Gimbal_camera_parameter.cy = request->cy;

    if(request->set_new_r_matrix)
    {
        Eigen::MatrixXd Ri(2,2);
        Ri.fill(0.0);
        Ri(0,0) = request->noise_cov_u;
        Ri(1,1) = request->noise_cov_v;
        response->success = AI_Co_Location_Computer.CreateNewAirplane(t_g,Pod_Rotation_Angles,AirplaneID,Gimbal_camera_parameter,Ri);
    }
    else
    {
        response->success = AI_Co_Location_Computer.CreateNewAirplane(t_g,Pod_Rotation_Angles,AirplaneID,Gimbal_camera_parameter);
    }
    // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\n AirplaneID: %ld" " Gimbal_camera_parameter.d: %f" " Gimbal_camera_parameter.f: %f",   // CHANGE
    //             AirplaneID, Gimbal_camera_parameter.d, Gimbal_camera_parameter.f);                                          // CHANGE
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Incoming request\n AirplaneID: %ld", AirplaneID);
}

/**
 * @brief 删除飞机节点
 *
 * 根据给定的飞机ID，从AI协同定位计算机中删除对应的飞机节点。
 *
 * @param request 包含飞机ID的请求数据
 * @param response 包含操作结果的响应数据
 */
void CoLocationNode::delete_airplane(const std::shared_ptr<client_interfaces::srv::DeleteAirplane::Request> request,
                                     std::shared_ptr<client_interfaces::srv::DeleteAirplane::Response> response)
{
    response->success = AI_Co_Location_Computer.DeleteAirplane(request->airplane_id);
}

/**
 * @brief 删除目标
 *
 * 删除指定ID的目标
 *
 * @param request 包含要删除的目标ID的请求对象
 * @param response 删除操作的结果对象
 */
void CoLocationNode::delete_target(const std::shared_ptr<client_interfaces::srv::DeleteTarget::Request> request,
                                   std::shared_ptr<client_interfaces::srv::DeleteTarget::Response> response)
{
    std::vector<uint8_t> fail_create_target_airplane;
    response->success = AI_Co_Location_Computer.DeleteTarget(fail_create_target_airplane, request->target_id);
}


/**
 * @brief 修改负载
 *
 * 该函数用于修改指定飞机的负载。
 *
 * @param request 包含请求信息的智能指针，包括飞机ID、负载类型、详细类型、负载1、负载2和负载3。
 * @param response 包含响应信息的智能指针，包括操作是否成功的标志。
 */
void CoLocationNode::change_payload(const std::shared_ptr<client_interfaces::srv::ChangePayload::Request> request,     // CHANGE
          std::shared_ptr<client_interfaces::srv::ChangePayload::Response>       response)  // CHANGE
{
  std::vector<double> Datas;
  Datas.emplace_back(request->payload1);
  Datas.emplace_back(request->payload2);
  Datas.emplace_back(request->payload3);
  response->success = AI_Co_Location_Computer.ChangeAirplanePayload(request->airplane_id,request->payload_type,request->detail_type,Datas);
}

/**
 * @brief 获取所有飞机的ID
 *
 * 从AI协同定位计算机中获取所有飞机的ID，并将它们存储在响应消息中。
 *
 * @param request 请求消息（未使用）
 * @param response 响应消息，包含所有飞机的ID
 */
void CoLocationNode::get_airplanes_id(const std::shared_ptr<client_interfaces::srv::GetAirplanesId::Request> ,     // CHANGE
          std::shared_ptr<client_interfaces::srv::GetAirplanesId::Response>       response)  // CHANGE
{
  std::vector<uint8_t> x = AI_Co_Location_Computer.GetAllAirplanesId();
  for(auto &it:x)
  {
    response->airplanes_id = {};
    response->airplanes_id.push_back(it);
  }
}

/**
 * @brief 获取所有目标节点的ID
 *
 * 从AI协同定位计算机中获取所有目标节点的ID，并将这些ID添加到响应中。
 *
 * @param request 请求参数，此参数在当前实现中未使用
 * @param response 响应参数，用于存储获取到的目标节点ID
 */
void CoLocationNode::get_targets_id(const std::shared_ptr<client_interfaces::srv::GetTargetsId::Request> ,     // CHANGE
          std::shared_ptr<client_interfaces::srv::GetTargetsId::Response>       response)  // CHANGE
{
  std::vector<uint8_t> x = AI_Co_Location_Computer.GetAllTargetsId();
  for(auto &it:x)
  {
    response->targets_id = {};
    response->targets_id.push_back(it);
  }
}